﻿#include"can_init.h"
#include"ip_can_fifo.h"
#include"can_intr_t.h"
#include"can_sys_t.h"
#include <unistd.h>
#include"ringbuffer.h"

#include "unistd.h"

#pragma pack(1)


#define CAN_SEND_SUCCESS       0
#define CAN_SEND_ERROR_HW      1
#define CAN_SEND_ERROR_BUSY    2
#define CAN_SEND_ERROR_PARAM   3

#define packet_size 48
#define CAN_MAX_DLC 8

#define CAN_PACK_LEN  48       // CAN 帧帧长
#define RW_BUFFER_LEN 4096 * 4 // 收发数据缓冲区最大长度

uint8_t buffer_recv[RW_BUFFER_LEN];    // 上位机接收的缓冲区数据
uint8_t buffer_can_recv[CAN_PACK_LEN]; // 从上位机发来的有效 CAN 帧数据
uint8_t buffer_can_send[CAN_PACK_LEN]; // 发送给上位机的有效 CAN 帧数据
uint8_t buffer_config_receive[CAN_PACK_LEN];
uint32_t flag=1;

#define CAN_RX_BUFFER_SIZE  2048

uint8_t channel;
uint8_t xor_status_calculated;

static u32 TxFrame[4];
u32 Can_TxFrame[4];


uint32_t total_frame_count = 0;

uint32_t channel_frame_count[6] = {0};

ring_buffer_t *  shared_can_rx_buffer;


typedef struct {
    uint32_t head;
    uint8_t direction;
    uint8_t type;
    uint8_t length;
    uint8_t ch_id;
    uint32_t total_count;
    uint32_t ch_count;
    uint32_t baud_rate;
    uint8_t mode;
    uint8_t auto_send;
    uint16_t ier;
    uint8_t reserved[12];
    uint64_t timestamp;
    uint32_t xor_checksum;
} data_frame1_t;

typedef struct {
    uint32_t head;
    uint8_t direction;
    uint8_t type;
    uint8_t length;
    uint8_t ch_id;
    uint32_t total_count;
    uint32_t ch_count;
    uint32_t idr;
    uint32_t drcr;
    uint32_t dw1r;
    uint32_t dw2r;
    uint16_t packets_sum;
    uint16_t packets_no;
    uint64_t timestamp;
    uint32_t xor_checksum;
} data_frame2_t;

typedef struct {
    uint32_t head;
    uint8_t direction;
    uint8_t type;
    uint8_t length;
    uint8_t ch_id;
    uint32_t total_count;
    uint32_t ch_count;
    uint32_t idr;
    uint32_t drcr;
    uint32_t dw1r;
    uint32_t dw2r;
    uint8_t states;
    uint8_t xor_states;
    uint16_t reserved2;
    uint64_t timestamp;
    uint32_t xor_checksum;
}data_frame3_t;

typedef struct {
    uint32_t head;
    uint8_t direction;
    uint8_t type;
    uint8_t length;
    uint8_t ch_id;
    uint32_t total_count;
    uint32_t ch_count;
    uint32_t idr;
    uint32_t drcr;
    uint32_t dw1r;
    uint32_t dw2r;
    uint32_t reserved;
    uint64_t timestamp;
    uint32_t xor_checksum;
}data_frame4_t;


typedef struct {
	uint32_t idr[1024];
	uint32_t drcr[1024];
	uint32_t dw1r[1024];
	uint32_t dw2r[1024];
    uint16_t received;
    uint8_t channel;
    uint8_t packet_map[256];
    uint16_t next_expected;
    uint8_t all_received;
    uint16_t total_sent;
    uint8_t sending;
} can_buffer_t;

can_buffer_t can_buffers[MAX_CAN_DEVICES];

uint32_t swap_endian_32bit(uint32_t value)
{
    return ((value & 0xFF000000) >> 24) |
           ((value & 0x00FF0000) >> 8)  |
           ((value & 0x0000FF00) << 8)  |
           ((value & 0x000000FF) << 24);
}

uint32_t calculate_xor_checksum(void *data, size_t total_bytes) {
    uint32_t checksum = 0;
    size_t dword_count = total_bytes / 4;

    for (size_t i = 0; i < dword_count; i++) {
        uint32_t current_dword;
        memcpy(&current_dword, data + i * 4, 4);
        checksum ^= current_dword;
    }

    if (total_bytes % 4 != 0) {
        uint32_t tail = 0;
        memcpy(&tail, data + dword_count * 4, total_bytes % 4);
        checksum ^= tail;
    }

    return checksum;
}


int SendFrame(drv_can_t *drv_can,  u32* Can_TxFrame)
{
    int status;


    if (drv_can == NULL ) {
        xil_printf("Error: SendFrame - Invalid parameters\r\n");
        return CAN_SEND_ERROR_PARAM;
    }

    int channel_id = -1;
    for (int i = 0; i < MAX_CAN_DEVICES; i++) {
        if (&can_system.drv_can[i] == drv_can) {
            channel_id = i;
            break;
        }
    }

    if (channel_id == -1) {
        xil_printf("Error: SendFrame - Cannot determine CAN channel ID\r\n");
        return CAN_SEND_ERROR_PARAM;
    }


    status = XCan_Send(&drv_can->can, Can_TxFrame);
    if (status == XST_SUCCESS)
    {

        return CAN_SEND_SUCCESS;
    }
    else
    {
    	return XST_FAILURE;
    }

}
void send_data_can(unsigned char *buffer, int length)
{
	data_frame2_t *frame = (data_frame2_t *)buffer;
	tx_data.id = frame->idr;
	tx_data.dlen = frame->drcr;
	tx_data.dw1 = frame->dw1r;
	tx_data.dw2 = frame->dw2r;
	tx_data.channel_id = frame->ch_id;

}

int send_data_to_can(unsigned char *buffer, int length)
{
	data_frame2_t *frame = (data_frame2_t *)buffer;

	uint32_t swap_can_drcr;

#if 0
	for(int i=0;i<length;i++)
		xil_printf("receive data %d 0X%02x\r\n", i, buffer[i]);
#endif
	if (frame->head != 0x499602D2)
	{
#if 0
		xil_printf("frame->head: 0X%08x\r\n",frame->head);
		xil_printf("Error: Invalid frame header\r\n");
#endif
		return XST_FAILURE;
	}
	uint32_t calculated_xor = calculate_xor_checksum(buffer, length - 4);

	if (calculated_xor != frame->xor_checksum)
	{
		xor_status_calculated = 0;
#if 1
		xil_printf("calculatef_xor: 0X%08x\r\n",calculated_xor);
		xil_printf("frame->xor_checksum: 0X%08x\r\n",frame->xor_checksum);
		xil_printf("recevice data calc error\r\n");
#endif
	}
	xor_status_calculated = 1;
	tx_data.id = frame->idr;
	tx_data.dlen = frame->drcr;
	tx_data.dw1 = frame->dw1r;
	tx_data.dw2 = frame->dw2r;
	tx_data.channel_id = frame->ch_id;
	//can_buffer_t *can_buf = &can_buffers[tx_data.channel_id];

//	if (frame->packets_sum > 1)
//	{
//		uint16_t packet_index = frame->packets_no;
//		if(packet_index>= frame->packets_sum)
//		{
//			xil_printf("Error: Invalid packet index\r\n");
//			return -2;
//		}
//		xil_printf("packet_index :0X%08x\r\n",packet_index);
//		xil_printf("can_buf->next_expected :0X%08x\r\n",can_buf->next_expected);
//		if (packet_index == can_buf->next_expected)
//		{
//		     can_buf->idr[packet_index] = frame->idr;
//		     can_buf->drcr[packet_index] = frame->drcr;
//		     can_buf->dw1r[packet_index] = frame->dw1r;
//		     can_buf->dw2r[packet_index] = frame->dw2r;
//		     can_buf->received++;
//		     can_buf->next_expected++;
//		     if (packet_index + 1 == frame->packets_sum)
//		     {
//		    	 can_buf->next_expected=0;
//
//		    	 for (uint8_t i = 0; i < frame->packets_sum; i++)
//		    	 {
//		    		 u32 Can_TxFrame[4];
//		    		 uint32_t swap_can_d;
//
//		    		 Can_TxFrame[0]=can_buf->idr[i];
//		    		 swap_can_d=swap_endian_32bit(can_buf->drcr[i]);
//		    		 Can_TxFrame[1]=XCan_CreateDlcValue(swap_can_d);
//		    		 Can_TxFrame[2]=can_buf->dw1r[i];
//		    		 Can_TxFrame[3]=can_buf->dw2r[i];
//					int send_status = SendFrame(&can_system.drv_can[tx_data.channel_id], Can_TxFrame);
//					if (send_status == CAN_SEND_SUCCESS)
//					{
//						//xil_printf("TEST CAN SEND SUCCESSFULL\r\n");
//						continue;
//					}
//					else
//					{
//						//xil_printf("TEST CAN SEND SUCCESSFULL (status=%d)\r\n", send_status);
//						continue;
//					}
//
//
//		    	 }
//
//		     }
//		}
//
//
//
//	}
//	else
//	{

		u32 Can_TxFrame[4];
		swap_can_drcr = swap_endian_32bit(frame->drcr);
		Can_TxFrame[0] = frame->idr;
		Can_TxFrame[1] = XCan_CreateDlcValue(swap_can_drcr);
		Can_TxFrame[2] = frame->dw1r;
		Can_TxFrame[3] = frame->dw2r;
		while (XCan_IsTxFifoFull(&can_system.drv_can[tx_data.channel_id].can) == TRUE);
		int send_status = SendFrame(&can_system.drv_can[tx_data.channel_id], Can_TxFrame);
	    if (send_status == CAN_SEND_SUCCESS)
	    {

	        return 1 ;
	    }
	    else
	    {

	        return 0 ;
	    }

	//}
	return XST_SUCCESS;
}

void send_can_tx_status(UINTPTR BaseAddress, int status, uint8_t ch_id) {
    static data_frame3_t status_frame =
    {
    		0x499602D2,
			0x01,
			0x80,
			0x30,
			0x00
    };
    status_frame.ch_id = ch_id;
    total_frame_count = (total_frame_count == 0xFFFFFFFF) ? 0 : total_frame_count + 1;
    status_frame.total_count = total_frame_count;


    channel_frame_count[status_frame.ch_id] = (channel_frame_count[status_frame.ch_id] == 0xFFFFFFFF) ? 0 : channel_frame_count[status_frame.ch_id] + 1;
    status_frame.ch_count = channel_frame_count[status_frame.ch_id];
    status_frame.idr=tx_data.id;
    status_frame.drcr=tx_data.dlen;
    status_frame.dw1r=tx_data.dw1;
    status_frame.dw2r=tx_data.dw2;
    status_frame.states = status;
    status_frame.xor_states=xor_status_calculated;//
    status_frame.reserved2 = 0;
    status_frame.timestamp = 0;
    status_frame.xor_checksum = calculate_xor_checksum((uint8_t *)&status_frame, sizeof(data_frame3_t) - 4);

    axis_fifo_write_data(BaseAddress,(uint8_t *)&status_frame,sizeof(data_frame3_t));
}

size_t can_read_data(int channel_id, can_rx_data_t *data, size_t elements)
{
	if (channel_id < 0 || channel_id >= MAX_CAN_DEVICES)
	{
		xil_printf("Error: Invalid CAN channel ID %d\r\n", channel_id);
		return 0;
    }
	drv_can_t *can_ptr = &can_system.drv_can[channel_id];
	if (!can_ptr->rx_buffer)
	{
		xil_printf("Error: No ring buffer assigned for channel %d\r\n", channel_id);
		return 0;
    }
	return ringbuffer_get(can_ptr->rx_buffer, data, elements);
}


int can_data_process(UINTPTR BaseAddress,can_rx_data_t *data)
{
	data_frame4_t axi_frame;
	int result = 0;

	axi_frame.head = 0x499602D2;
	axi_frame.direction = 1;
	axi_frame.type = 0x81;
	axi_frame.length =0x30;
	axi_frame.ch_id = data->channel_id;
	total_frame_count = (total_frame_count == 0xFFFFFFFF) ? 0 : total_frame_count + 1;
	axi_frame.total_count = total_frame_count;
    channel_frame_count[axi_frame.ch_id] = (channel_frame_count[axi_frame.ch_id] == 0xFFFFFFFF) ? 0 : channel_frame_count[axi_frame.ch_id] + 1;
    axi_frame.ch_count = channel_frame_count[axi_frame.ch_id];
	axi_frame.idr =data->id;
	axi_frame.drcr=data->dlen;
	axi_frame.dw1r=data->dw1;
	axi_frame.dw2r=data->dw2;
	axi_frame.reserved = 0;
	axi_frame.timestamp=0;
	axi_frame.xor_checksum=calculate_xor_checksum(&axi_frame, sizeof(data_frame4_t) - 4);

	result=axis_fifo_write_data(BaseAddress,(uint8_t *)&axi_frame,sizeof(data_frame4_t));
	if (result < 0)
	{
	      xil_printf("deault: AXI FIFO write error  (: %d)\r\n", result);
	      return 0;
	}
	else if (result != sizeof(data_frame4_t))
	{
	      xil_printf("waring: other data can'n write AXI FIFO (write: %d, expect: %d)\r\n",result, (int)sizeof(data_frame4_t));
	      return -1;
	 }

	return 0;

}

/*************************************************************************************/

void parse_and_init_can(unsigned char *buffer, int length) {

    if (length < sizeof(data_frame1_t)) {
        return;
    }

    data_frame1_t *frame = (data_frame1_t *)buffer;

    if (frame->head != 0x499602D2) {
        return;
    }
    xil_printf("head  0x%08X\r\n", frame->head);

    uint32_t expected_checksum = calculate_xor_checksum((uint8_t *)frame, sizeof(data_frame1_t)-4);
    if (frame->xor_checksum != expected_checksum) {
        return;
    }

    xil_printf("xor_checksum  0x%08X\r\n", frame->xor_checksum);
    xil_printf("type 0x%02X\r\n",frame->type);
    if (frame->direction == 0)
    {
    	switch (frame->type) {
    	case 0x00:

    		     CAN_SetBaudRate(&can_system.drv_can[frame->ch_id], frame->baud_rate);

    		     CAN_SetMode(&can_system.drv_can[frame->ch_id], frame->mode);
    		     xil_printf("CAN%d configured: baud=%d, mode=%d\r\n", frame->ch_id, frame->baud_rate, frame->mode);
    		     axis_fifo_reset(AXIS_FIFO_BASEADDRESS);
    		    break ;

    	case 0x01:

    		break;

    	}
    }
}

int create_ring_buffers(void)
{
	shared_can_rx_buffer=ringbuffer_create(CAN_RX_BUFFER_SIZE * MAX_CAN_DEVICES, sizeof(can_rx_data_t));
	if (shared_can_rx_buffer == NULL)
	{
		xil_printf("Error: Failed to create shared ring buffer\r\n");
		return XST_FAILURE;
	}
	for (int i = 0; i < MAX_CAN_DEVICES; i++)
	{
		can_system.drv_can[i].rx_buffer = shared_can_rx_buffer;
		can_system.drv_can[i].count = 0;
	}
	return XST_SUCCESS;
}



int main(void)
{
    int status;
    status = system_initialize();
    if (status != XST_SUCCESS)
    {
        xil_printf("System Initialize Failed! Code: 0x%08x\r\n", status);
        return status;
    }
    status = create_ring_buffers();
    if (status != XST_SUCCESS)
    {
        xil_printf("Ring buffer creation failed!\r\n");
        return status;
    }


    while(1)
    {
        int data_config_len = axis_fifo_read_data_len(CONFIG_FIFO_BASEADDRESS);
        int data_axi_len = axis_fifo_read_data_len(AXIS_FIFO_BASEADDRESS);
        data_axi_len = data_axi_len / 48 * 48;

#if 1
        for (int i = 0; i < MAX_CAN_DEVICES; i++)
        {
        	if (can_read_data(i, &rx_data, 1) > 0)
        	{
        		can_data_process(AXIS_FIFO_BASEADDRESS,&rx_data);
        	}
        }

#endif
        if (data_axi_len > 0)
        {
#if 0 // enable print
        	xil_printf("%d\r\n", data_axi_len);
#endif
			int read_length = axis_fifo_read_data(AXIS_FIFO_BASEADDRESS, buffer_recv, data_axi_len);
			for (uint16_t i = 0; i < read_length; i += CAN_PACK_LEN)
			{
#if 1 // enable to send to Can
			status = send_data_to_can(buffer_recv + i, CAN_PACK_LEN);
#endif
#if 1 // enable read back can
			// must call send_data_to_can() first.
			send_can_tx_status(AXIS_FIFO_BASEADDRESS, status, tx_data.channel_id);
#endif
			}
#if 0 // enable read back origin data
			axis_fifo_write_data(AXIS_FIFO_BASEADDRESS, buffer_recv, read_length);
#endif
        }

#if 1   //receive CONFIG_fifo data
        if(data_config_len> 0 && data_config_len==sizeof(data_frame1_t))
        {
        	int read_length = axis_fifo_read_data(CONFIG_FIFO_BASEADDRESS, buffer_config_receive, sizeof(data_frame1_t));
        	parse_and_init_can( buffer_config_receive,read_length);
        }
#endif


    }

    return XST_SUCCESS;
}



